#include "system_interface.h"

#define WAIT_FOR_FLAG(flag, timeout, errorcode) Timeout = timeout;\
            while(!flag)  {\
              if((Timeout--) == 0) return errorcode; \
            }\

int spi_master_write_register (uint8_t register_addr, uint16_t len, const uint8_t *value);

int inv_io_hal_read_reg(struct inv_icm426xx_serif * serif, uint8_t reg, uint8_t * rbuffer, uint32_t rlen)
{
	//Use 4 line SPI on ICMSPI


	uint32_t Timeout = 0;
	int i = 0;
	
	ICMNSS_ENABLE;
	
	//Wait until the spi Tx buffer is empty
	WAIT_FOR_FLAG (LL_SPI_IsActiveFlag_TXE (ICMSPI),1000,4);	
	LL_SPI_TransmitData8 (ICMSPI, reg|0x80); //Write register address
	//Wait until the api RX buffer is not empty
	WAIT_FOR_FLAG (LL_SPI_IsActiveFlag_RXNE (ICMSPI),1000,5);
	LL_SPI_ReceiveData8 (ICMSPI); //Clear RXNE bit
	
	for ( i = 0; i < rlen; i++)
	{
		WAIT_FOR_FLAG (LL_SPI_IsActiveFlag_TXE (ICMSPI),1000,6);
		LL_SPI_TransmitData8 (ICMSPI, 0x00); //Dummy byte ro genarate clock
		WAIT_FOR_FLAG (LL_SPI_IsActiveFlag_RXNE (ICMSPI),1000,7);
		rbuffer[i] = LL_SPI_ReceiveData8 (ICMSPI);
	}
	ICMNSS_DISABLE; 

	return 0;
	
//		uint8_t datout = 0;

//	while((ICMSPI->SR & 1<<1) == 0);
//		LL_SPI_TransmitData8 (ICMSPI, dat); //Dummy byte ro genarate clock

//	
//	while((ICMSPI->SR & 1<<0) == 0);

//		datout = LL_SPI_ReceiveData8 (ICMSPI);

//	return datout;
}


int inv_io_hal_write_reg (struct inv_icm426xx_serif * serif, uint8_t reg, const uint8_t * wbuffer, uint32_t wlen)
{
    int rc = 0;

    for (uint32_t i = 0; i < wlen; i++)
    {
        rc = spi_master_write_register (reg+i, 1, &wbuffer[i]);
        if(rc)
            return rc;
    }

	return 0;
}

int spi_master_write_register (uint8_t register_addr, uint16_t len, const uint8_t *value)
{

    //Use 4 line SPI on ICMSPI

	uint32_t Timeout = 0;
	uint8_t rec = 0;
	
	ICMNSS_ENABLE;

    WAIT_FOR_FLAG (LL_SPI_IsActiveFlag_TXE(ICMSPI),1000,0);
    LL_SPI_TransmitData8 (ICMSPI, register_addr);
	WAIT_FOR_FLAG (LL_SPI_IsActiveFlag_RXNE(ICMSPI),1000,1);
	LL_SPI_ReceiveData8 (ICMSPI); //Clear RXNE bit
//	HAL_SPI_TransmitReceive(&hspi3,&register_addr,&rec,1,1000);

    for (int i = 0; i < len; i++)
    {
		WAIT_FOR_FLAG (LL_SPI_IsActiveFlag_TXE(ICMSPI),1000,2);
		LL_SPI_TransmitData8(ICMSPI, value[i]); //Dummy byte ro genarate clock
		WAIT_FOR_FLAG (LL_SPI_IsActiveFlag_RXNE(ICMSPI),1000,3);
		LL_SPI_ReceiveData8(ICMSPI);

    }

    ICMNSS_DISABLE; 
    return 0;
}


static uint32_t sDisableIntCount = 0;


void disable_irq(void)
{
	if(sDisableIntCount == 0) {
		__disable_irq();
	}
	sDisableIntCount ++;
}

void enable_irq(void)
{
	sDisableIntCount --;
	if(sDisableIntCount == 0) {
		__enable_irq();
	}
}


